using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;

namespace OPEN_ROBOT_TCP_GUI
{
    /// <summary>
    /// OPEN_ROBOT_TCP_GUI Form Class. Contains all the controls to manually operate an A.B.E. equipped robot.
    /// </summary>
    public partial class OPEN_ROBOT_TCP_GUI_Form : Form
    {
        /// <summary>
        /// Declare an instance of the OPEN_ROBOTbot Functions Library.
        /// </summary>
        OPEN_ROBOT_TCP_Class_Library.OPEN_ROBOT_TCP_Functions OPEN_ROBOT_Lib = new OPEN_ROBOT_TCP_Class_Library.OPEN_ROBOT_TCP_Functions();

        /// <summary>
        /// Set the number of serial port read retries.
        /// </summary>
        int numRetries = 0;

        /// <summary>
        /// Command timeout in milliseconds (ms).
        /// </summary>
        int commandTimeOut = 1000;

        /// <summary>
        /// Maximum WiFi transmission time from WiFi Test.
        /// </summary>
        int maxWiFiTime = 0;

        /// <summary>
        /// Number of WiFi timing samples from WiFi Test.
        /// </summary>
        int numWiFiSamples = 0;

        /// <summary>
        /// Application directory.
        /// </summary>
        string strApplicationDir = "";

        /// <summary>
        /// Wheel Center Distance.
        /// </summary>
        double dblWheelCenterDistance = 4.375;

        /// <summary>
        /// Image Number is used to write Jpeg image to file.
        /// </summary>
        int imageNumber = 0;

        /// <summary>
        /// YUV444 Image Structure.
        /// </summary>
        struct YUV444
        {
            public int Y;
            public int U;
            public int V;
        }

        /// <summary>
        /// RGB Structure.
        /// </summary>
        struct RGB
        {
            public int R;
            public int G;
            public int B;
        }
        
        /// <summary>
        /// Thread to read PIC18F4520 hex file and upload to robot.
        /// </summary>
        Thread hexDataReader;

        string picHexFileName, hexUploadError;

        /// <summary>
        /// OPEN_ROBOT_TCP GUI Form Constructor.
        /// </summary>
        public OPEN_ROBOT_TCP_GUI_Form()
        {
            InitializeComponent();
        }

        /// <summary>
        /// Function to exit application.
        /// </summary>
        public void Exit_Application()
        {
            OPEN_ROBOT_Lib.ClearSocketBuffer();
            Application.Exit();
        }

        /// <summary>
        /// File Exit menu event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void mnuFile_Exit_Click(object sender, EventArgs e)
        {
            Exit_Application();
        }

        /// <summary>
        /// OPEN_ROBOT_TCP_GUI Form Load Event
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void OPEN_ROBOT_TCP_GUI_Form_Load(object sender, EventArgs e)
        {
            strApplicationDir = System.Environment.CurrentDirectory;
        }

        /// <summary>
        /// Send Motion Command Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSendMotionCommand_Click(object sender, EventArgs e)
        {
            int leftMotor, rightMotor;

            try
            {
                leftMotor = Convert.ToInt16(nmbLeftMotor.Value);
                rightMotor = Convert.ToInt16(nmbRightMotor.Value);

                if (rdoOpenLoopVelocity.Checked)
                {
                    OPEN_ROBOT_Lib.SetOpenLoopVelocity(leftMotor, rightMotor, commandTimeOut, numRetries);
                }
                else if (rdoClosedLoopVelocity.Checked)
                {
                    OPEN_ROBOT_Lib.SetClosedLoopVelocity(leftMotor, rightMotor, commandTimeOut, numRetries);
                }
                else if (rdoPosition.Checked)
                {
                    OPEN_ROBOT_Lib.MoveToPosition(leftMotor, rightMotor, commandTimeOut, numRetries);
                }
                else return;

                Add_Message("Move Command Recieved");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Stop Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bStop_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.StopRobot(commandTimeOut, numRetries);
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
            
            Add_Message("Robot Stopped");
        }

        /// <summary>
        /// Is Move Complete Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bIsMoveComplete_Click(object sender, EventArgs e)
        {
            try
            {
                bool moveComplete = OPEN_ROBOT_Lib.IsMoveComplete(commandTimeOut, numRetries);

                if (moveComplete) Add_Message("Move Complete");
                else Add_Message("Move Not Complete");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Reset RFID Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bResetRFID_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.ResetRfidModule(commandTimeOut, numRetries);

                Add_Message("RFID Reset");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Write RFID Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bWriteRFID_Click(object sender, EventArgs e)
        {
            try
            {
                if (rdoRawMode.Checked)
                {
                    OPEN_ROBOT_Lib.WriteRfidDataASCII(txtRFIDascii.Text, commandTimeOut, numRetries);
                }
                else if (rdoHexMode.Checked)
                {
                    OPEN_ROBOT_Lib.WriteRfidDataHex(txtRFIDhex.Text, commandTimeOut, numRetries);
                }

                Add_Message("RFID Data Written");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Read RFID Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bReadRFID_Click(object sender, EventArgs e)
        {
            try
            {
                string rfidData = string.Empty;

                if (rdoRawMode.Checked)
                {
                    rfidData = OPEN_ROBOT_Lib.GetAsciiRfidReading(commandTimeOut, numRetries);
                    Add_Message("RFID ASCII = " + rfidData);
                }
                else if (rdoHexMode.Checked)
                {
                    rfidData = OPEN_ROBOT_Lib.GetHexRfidReading(commandTimeOut, numRetries);
                    Add_Message("RFID HEX = " + rfidData);
                }
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Set Reflex Level Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSetReflexLevel_Click(object sender, EventArgs e)
        {
            try
            {
                if (rdoADreflex.Checked)
                {
                    OPEN_ROBOT_Lib.SetAnalogToDigitalReflexLevel(Convert.ToInt16(nmbReflexLevel.Value), commandTimeOut, numRetries);
                }
                else
                {
                    OPEN_ROBOT_Lib.SetLightReflexLevel(Convert.ToInt16(nmbReflexLevel.Value), commandTimeOut, numRetries);
                }

                Add_Message("Reflex Set");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get Reflex Level Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetReflexLevel_Click(object sender, EventArgs e)
        {
            try
            {
                byte[] reflexLevels = OPEN_ROBOT_Lib.GetReflexLevels(commandTimeOut, numRetries);
                
                if (rdoADreflex.Checked)
                {
                    Add_Message("AD Reflex Level = " + reflexLevels[0].ToString());
                    nmbReflexLevel.Value = reflexLevels[0];
                }
                else
                {
                    Add_Message("Light Reflex Level = " + reflexLevels[1].ToString());
                    nmbReflexLevel.Value = reflexLevels[1];
                }
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Reset Reflex Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bResetReflex_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.ResetSensorReflexTrigger(commandTimeOut, numRetries);
                
                Add_Message("Reflex Reset");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Is Reflex Triggered Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bReflexTriggered_Click(object sender, EventArgs e)
        {
            try
            {
                bool reflexTriggered = OPEN_ROBOT_Lib.IsReflexTriggered(commandTimeOut, numRetries);
                if (reflexTriggered) Add_Message("Reflex Triggered");
                else Add_Message("Reflex Not Triggered");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Set Sensor Reflex Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSetSensorReflex_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SetSensorReflexes(txtSensorReflexes.Text, commandTimeOut, numRetries);
                
                Add_Message("Sensor Reflexes Set");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get Wheel Encoder Readings Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetEncoders_Click(object sender, EventArgs e)
        {
            try
            {
                int[] encoderReadings = OPEN_ROBOT_Lib.GetWheelEncoderReadings(commandTimeOut, numRetries);

                Add_Message("Right Encoder = " + encoderReadings[0].ToString());
                Add_Message("Left Encoder = " + encoderReadings[1].ToString());
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get Wheel Velocity Raedings Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetWheelVelocities_Click(object sender, EventArgs e)
        {
            try
            {
                int[] wheelVelocities = OPEN_ROBOT_Lib.GetWheelVelocities(commandTimeOut, numRetries);

                Add_Message("Right Wheel Velocity = " + wheelVelocities[0].ToString());
                Add_Message("Left Wheel Velocity = " + wheelVelocities[1].ToString());
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get AD Raedings Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetADReadings_Click(object sender, EventArgs e)
        {
            try
            {
                byte[] strResp = OPEN_ROBOT_Lib.GetAnalogToDigitalSensorReadings(commandTimeOut, numRetries);

                if (strResp.Length != 8)
                {
                    Add_Message("ERROR READING FROM ROBOT!");
                    return;
                }

                Add_Message("FRL = " + strResp[6].ToString());
                Add_Message("FLL = " + strResp[7].ToString());
                Add_Message("RRIR = " + strResp[5].ToString());
                Add_Message("RLIR = " + strResp[4].ToString());
                Add_Message("FRIR = " + strResp[3].ToString());
                Add_Message("FLIR = " + strResp[2].ToString());
                Add_Message("FCIR = " + strResp[1].ToString());
                Add_Message("Battery = " + System.String.Format("{0:#.00}", Convert.ToInt16(strResp[0]) * 37 / 688.5) + " Volts");

                if (strResp[0] < 130) Add_Message("Batteries are low and require charging or replacement!");

            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Set PID Gains Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSetPIDgains_Click(object sender, EventArgs e)
        {
            int Kp, Ki, Kd;

            Kp = Convert.ToByte(nmbKp.Value);
            Ki = Convert.ToByte(nmbKi.Value);
            Kd = Convert.ToByte(nmbKd.Value);

            try
            {
                if (rdoPositionPID.Checked) OPEN_ROBOT_Lib.SetPositionGainsPID(Kp, Ki, Kd, commandTimeOut, numRetries);
                else OPEN_ROBOT_Lib.SetVelocityGainsPID(Kp, Ki, Kd, commandTimeOut, numRetries);
                
                Add_Message("PID Gains Set");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get PID Gains Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetPIDgains_Click(object sender, EventArgs e)
        {
            byte[] pidGains;

            if (rdoPositionPID.Checked) pidGains = OPEN_ROBOT_Lib.GetPositionGainsPID(commandTimeOut, numRetries);
            else pidGains = OPEN_ROBOT_Lib.GetVelocityGainsPID(commandTimeOut, numRetries);

            nmbKp.Value = pidGains[0];
            nmbKi.Value = pidGains[1];
            nmbKd.Value = pidGains[2];

            Add_Message("Kd = " + nmbKd.Value.ToString());
            Add_Message("Ki = " + nmbKi.Value.ToString());
            Add_Message("Kp = " + nmbKp.Value.ToString());
        }

        /// <summary>
        /// Set Pin High Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSetPinHigh_Click(object sender, EventArgs e)
        {
            try
            {
                int pinNum = Convert.ToByte(nmbPinNumber.Value);

                OPEN_ROBOT_Lib.SetDigitalPinState(pinNum, true, commandTimeOut, numRetries);
                
                Add_Message("Pin Set High");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Set Pin Low Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSetPinLow_Click(object sender, EventArgs e)
        {
            try
            {
                int pinNum = Convert.ToByte(nmbPinNumber.Value);

                OPEN_ROBOT_Lib.SetDigitalPinState(pinNum, false, commandTimeOut, numRetries);

                Add_Message("Pin Set Low");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get Pin Reading Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bGetPinReading_Click(object sender, EventArgs e)
        {
            try
            {
                int pinNum = Convert.ToByte(nmbPinNumber.Value);

                bool pinState = OPEN_ROBOT_Lib.GetDigitalPinState(pinNum, commandTimeOut, numRetries);

                if (pinState) Add_Message("Pin State = High");
                else Add_Message("Pin State = Low");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Save To EEPROM Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bSaveToEEPROM_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SaveParametersToEEPROM(commandTimeOut, numRetries);
                
                Add_Message("Data Saved");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Clear Command Window Button Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void bClearCommandWindow_Click(object sender, EventArgs e)
        {
            txtData.Text = "";
            txtVisionData.Text = "";
        }

        /// <summary>
        /// Clear Vision Command Window Button Event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void btnClearVisionWindow_Click(object sender, EventArgs e)
        {
            txtVisionData.Text = "";
            txtData.Text = "";
        }

        /// <summary>
        /// Function to Add a Message to the Command Window.
        /// </summary>
        /// <param name="strMsg">Message string to add to Command Window.</param>
        private void Add_Message(string strMsg)
        {
            string strTmp = txtData.Text;
            txtData.Text = strMsg + "\r\n" + strTmp;
            txtVisionData.Text = strMsg + "\r\n" + strTmp;
        }

        /// <summary>
        /// Help Menu Manual Event Click Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void mnuHelp_Manual_Click(object sender, EventArgs e)
        {
            string strFilePath = strApplicationDir + "\\" + "ABEbot_Control_Center_Manual.pdf";
            try
            {
                System.Diagnostics.Process.Start(strFilePath);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message.ToString());
            }
        }

        /// <summary>
        /// Help Menu Online Support Click Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void mnuHelp_OnlineSupport_Click(object sender, EventArgs e)
        {
            try
            {
                System.Diagnostics.Process.Start("www.abotics.com/open_robot.htm");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message.ToString());
            }
        }

        /// <summary>
        /// Help Menu Email Support Click Event.
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        private void mnuEmailSupport_Click(object sender, EventArgs e)
        {
            try
            {
                System.Diagnostics.Process.Start("mailto:abe@abotics.com");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message.ToString());
            }
        }

        private void bConvertToInches_Click(object sender, EventArgs e)
        {
            string strInches = String.Format("{0:#.00}", Convert.ToInt16(nmbTicks.Value) * 0.064);
            Add_Message("Ticks to Inches = " + strInches);
        }

        private void bConvertToDegrees_Click(object sender, EventArgs e)
        {
            string strDegrees = String.Format("{0:#.00}", Convert.ToInt16(nmbTicks.Value) * 2.621 * 2 * 180 / (dblWheelCenterDistance * 128));
            Add_Message("Ticks to Degrees = " + strDegrees);
        }

        /// <summary>
        /// Wander Mode Button Event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bWander_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.StartWanderMode(commandTimeOut, numRetries);
                
                Add_Message("Entered Wander Mode");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Find Tag Event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bFindTag_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.FindRfidTag(commandTimeOut, numRetries);
                
                Add_Message("Searching for RFID Tags");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Tag Found Event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bTagFound_Click(object sender, EventArgs e)
        {
            try
            {
                bool tagFound = OPEN_ROBOT_Lib.IsRfidTagFound(commandTimeOut, numRetries);
                if (tagFound) Add_Message("Tag Found");
                else Add_Message("No Tag Found");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Stop Searching for RFID tags.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bStopTagSearch_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.StopRfidTagSearch(commandTimeOut, numRetries);
                Add_Message("Tag Search Terminated");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Try to create and open tcp-socket connection wth OPEN-ROBOT.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void menuSocketOpen_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.OpenSocketConnection(txtIP.Text, Convert.ToInt16(txtPort.Text));
                lblIP.Text = "Socket Open";
                Add_Message("Socket Open");
                menuSocketClose.Enabled = true;
                menuSocketOpen.Enabled = false;
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Close the tcp-socket connection.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void menuSocketClose_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.CloseSocketConnection();
                lblIP.Text = "Socket Closed";
                menuSocketClose.Enabled = false;
                menuSocketOpen.Enabled = true;
                Add_Message("Socket Closed");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Attempt to get a JPEG image from the SRV1 Camera Board.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void getJPEG_Click(object sender, EventArgs e)
        {
            // Check to make sure image directory exisits.
            if (Directory.Exists(txtImageDirectory.Text) == false)
            {
                // Create the directory.
                Directory.CreateDirectory(txtImageDirectory.Text);
            }

            try
            {
                int startTime = System.Environment.TickCount;

                byte[] imageBytes = OPEN_ROBOT_Lib.GetJpegImage((int)nmbImageTimeOut.Value);

                double imageTime = (double)(System.Environment.TickCount - startTime);

                lblImageTime.Text = "Image Time = " + imageTime.ToString() + "ms";

                if (imageBytes.Length > 1)
                {
                    string fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot" + imageNumber.ToString() + ".jpg";
                    imageNumber++;
                    FileStream fs = new FileStream(fileNamePath, FileMode.OpenOrCreate);
                    BinaryWriter bw = new BinaryWriter(fs);
                    for (int i = 0; i < imageBytes.Length; i++)
                    {
                        bw.Write(imageBytes[i]);
                    }
                    bw.Close();
                    srv1PictureBox.ImageLocation = fileNamePath;
                }
                else
                {
                    if (imageBytes[0] == 1)
                    {
                        Add_Message("SOCKET_CLOSED");
                    }
                    else Add_Message("Error Retrieving JPEG.");
                    return;
                }
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
                return;
            }

            Add_Message("JPEG Image Retieved.");
        }

        /// <summary>
        /// Set the SRV1 Camera JEPG image size.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void setJpegSize_Click(object sender, EventArgs e)
        {
            string strData = "";

            if (rdo160x120.Checked) strData = "160x128";
            else if (rdo320x240.Checked) strData = "320x256";
            else if (rdo640x480.Checked) strData = "640x512";
            else if (rdo1280x1024.Checked) strData = "1280x1024";

            try
            {
                OPEN_ROBOT_Lib.SetSRV1CameraResolution(strData, commandTimeOut);
                Add_Message("SRV1 Camera resolution set.");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }

        }

        /// <summary>
        /// Image timer to collect and save images to file.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void imageTimer_Tick(object sender, EventArgs e)
        {
            imageTimer.Stop();

            int startTime = System.Environment.TickCount;

            byte[] imageBytes = OPEN_ROBOT_Lib.GetJpegImage((int)nmbImageTimeOut.Value);

            double imageTime = (double)(System.Environment.TickCount - startTime);

            lblImageTime.Text = "Image Time = " + imageTime.ToString() + "ms";

            if (imageBytes.Length > 1)
            {
                string fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot" + imageNumber.ToString() + ".jpg";
                imageNumber++;
                FileStream fs = new FileStream(fileNamePath, FileMode.OpenOrCreate);
                BinaryWriter bw = new BinaryWriter(fs);
                for (int i = 0; i < imageBytes.Length; i++)
                {
                    bw.Write(imageBytes[i]);
                }
                bw.Close();
                srv1PictureBox.ImageLocation = fileNamePath;
            }
            else
            {
                if (imageBytes[0] == 1)
                {
                    Add_Message("SOCKET_CLOSED");
                }
                else Add_Message("Error Retrieving JPEG.");
                return;
            }
            imageTimer.Start();
        }

        /// <summary>
        /// Checkbox to acquire SRV1 Camera images continuously.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void ckbAcquireImages_CheckedChanged(object sender, EventArgs e)
        {
            if (ckbAcquireImages.Checked)
            {
                // Check to make sure image directory exisits.
                if (Directory.Exists(txtImageDirectory.Text) == false)
                {
                    // Create the directory.
                    Directory.CreateDirectory(txtImageDirectory.Text);
                }
                imageTimer.Start();
            }
            else imageTimer.Stop();
        }

        /// <summary>
        /// Set the SRV1 Camera Board JPEG quality.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void setJpegQuality_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SetJpegQuality((int)numJpegQuality.Value, commandTimeOut);
                Add_Message("SRV1 Jpeg quality set.");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Get Raw YUV image data. Requires special SRV1 Camera Board firmware change!
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void getRawYUV_Click(object sender, EventArgs e)
        {
            // Check to make sure image directory exisits.
            if (Directory.Exists(txtImageDirectory.Text) == false)
            {
                // Create the directory.
                Directory.CreateDirectory(txtImageDirectory.Text);
            }

            int numBytes = 0;
            int imageWidth = 0;
            int imageHeight = 0;

            if (rdo160x120.Checked)
            {
                numBytes = 160 * 120 * 2;
                imageWidth = 160;
                imageHeight = 120;
            }
            else if (rdo320x240.Checked)
            {
                numBytes = 320 * 240 * 2;
                imageWidth = 320;
                imageHeight = 240;
            }
            else if (rdo640x480.Checked)
            {
                numBytes = 640 * 480 * 2;
                imageWidth = 640;
                imageHeight = 480;
            }
            else if (rdo1280x1024.Checked)
            {
                numBytes = 1280 * 1024 * 2;
                imageWidth = 1280;
                imageHeight = 1024;
            }

            int startTime = System.Environment.TickCount;

            byte[] imageBytes = OPEN_ROBOT_Lib.GetRawImage((int)nmbImageTimeOut.Value, numBytes);

            double imageTime = (double)(System.Environment.TickCount - startTime) / 1000;

            lblImageTime.Text = "Image Time = " + imageTime.ToString();

            Add_Message("YUV Image Acquired.");

            if (imageBytes.Length == numBytes)
            {
                string fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_YUV" + ".txt";
                FileStream fs = new FileStream(fileNamePath, FileMode.Create);
                BinaryWriter bw = new BinaryWriter(fs);
                for (int i = 0; i < imageBytes.Length; i++)
                {
                    bw.Write(imageBytes[i]);
                }
                bw.Close();

                // Write the data out as comma delimited int8.
                fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_YUV_CVS" + ".txt";
                fs = new FileStream(fileNamePath, FileMode.Create);
                StreamWriter sw = new StreamWriter(fs);
                for (int i = 0; i < imageBytes.Length; i++)
                {
                    sw.Write(Convert.ToInt16(imageBytes[i]).ToString());
                    if (i < imageBytes.Length - 1) sw.Write(',');
                }
                sw.Close();

                //Write out intensity image.
                fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_intensity" + ".txt";
                fs = new FileStream(fileNamePath, FileMode.Create);
                sw = new StreamWriter(fs);
                int columnIndex = 0;

                for (int i = 1; i < imageBytes.Length; i += 2)
                {
                    sw.Write(Convert.ToInt16(imageBytes[i]).ToString());
                    columnIndex++;
                    if (columnIndex >= imageWidth)
                    {
                        columnIndex = 0;
                        sw.Write(System.Environment.NewLine);
                    }
                    else if (i < imageBytes.Length - 1) sw.Write(',');
                }
                sw.Close();

                // Convert to YUV444 then to RGB.
                YUV444[] yuv444 = new YUV444[imageHeight * imageWidth];
                int yuvIndex = 0;

                for (int i = 3; i < imageBytes.Length; i += 4)
                {
                    yuv444[yuvIndex].Y = imageBytes[i - 2];
                    yuv444[yuvIndex].U = imageBytes[i - 3];
                    yuv444[yuvIndex].V = imageBytes[i - 1];
                    yuvIndex++;
                    yuv444[yuvIndex].Y = imageBytes[i];
                    yuv444[yuvIndex].U = imageBytes[i - 3];
                    yuv444[yuvIndex].V = imageBytes[i - 1];
                    yuvIndex++;
                }

                // Convert to RGB image.
                RGB[] rgbImage = new RGB[yuv444.Length];

                for (int i = 0; i < rgbImage.Length; i++)
                {
                    double y, u, v, r, g, b;
                    y = yuv444[i].Y;
                    u = yuv444[i].U;
                    v = yuv444[i].V;

                    r = y + 1.370705 * (v - 128);
                    g = y - 0.698001 * (v - 128) - 0.337633 * (u - 128);
                    b = y + 1.732446 * (u - 128);
                    if (r < 0) r = 0;
                    if (r > 255) r = 255;
                    if (g < 0) g = 0;
                    if (g > 255) g = 255;
                    if (b < 0) b = 0;
                    if (b > 255) b = 255;
                    rgbImage[i].R = (int)r;
                    rgbImage[i].G = (int)g;
                    rgbImage[i].B = (int)b;
                }
                // Write out comma delimited RGB data to three seperate files.
                fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_red" + ".txt";
                fs = new FileStream(fileNamePath, FileMode.Create);
                sw = new StreamWriter(fs);
                columnIndex = 0;

                for (int i = 0; i < rgbImage.Length; i++)
                {
                    sw.Write(rgbImage[i].R.ToString());
                    columnIndex++;
                    if (columnIndex >= imageWidth)
                    {
                        columnIndex = 0;
                        sw.Write(System.Environment.NewLine);
                    }
                    else if (i < rgbImage.Length - 1) sw.Write(',');
                }
                sw.Close();

                fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_green" + ".txt";
                fs = new FileStream(fileNamePath, FileMode.Create);
                sw = new StreamWriter(fs);
                columnIndex = 0;

                for (int i = 0; i < rgbImage.Length; i++)
                {
                    sw.Write(rgbImage[i].G.ToString());
                    columnIndex++;
                    if (columnIndex >= imageWidth)
                    {
                        columnIndex = 0;
                        sw.Write(System.Environment.NewLine);
                    }
                    else if (i < rgbImage.Length - 1) sw.Write(',');
                }
                sw.Close();

                fileNamePath = txtImageDirectory.Text + "\\srv1_open_robot_blue" + ".txt";
                fs = new FileStream(fileNamePath, FileMode.Create);
                sw = new StreamWriter(fs);
                columnIndex = 0;

                for (int i = 0; i < rgbImage.Length; i++)
                {
                    sw.Write(rgbImage[i].B.ToString());
                    columnIndex++;
                    if (columnIndex >= imageWidth)
                    {
                        columnIndex = 0;
                        sw.Write(System.Environment.NewLine);
                    }
                    else if (i < rgbImage.Length - 1) sw.Write(',');
                }
                sw.Close();

                imageNumber++;
            }
            else
            {
                Add_Message("ERROR!");
            }
        }

        /// <summary>
        /// Move robot forward from Vision tab.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bForward_Click(object sender, EventArgs e)
        {
            int motorSpeed;

            motorSpeed = Convert.ToInt16(nmbMotorSpeed.Value);

            try
            {
                OPEN_ROBOT_Lib.SetOpenLoopVelocity(motorSpeed, motorSpeed, commandTimeOut, numRetries);

                Add_Message("Move Command Recieved");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Move robot back from Vision tab.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bBackward_Click(object sender, EventArgs e)
        {
            int motorSpeed;

            motorSpeed = Convert.ToInt16(nmbMotorSpeed.Value);

            try
            {
                OPEN_ROBOT_Lib.SetOpenLoopVelocity(-motorSpeed, -motorSpeed, commandTimeOut, numRetries);

                Add_Message("Move Command Recieved");
            }
            catch (Exception ex)
            {
                Add_Message (ex.ToString());
            }
        }

        /// <summary>
        /// Spin robot right from Vision tab.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bSpinRight_Click(object sender, EventArgs e)
        {
            int motorSpeed;

            motorSpeed = Convert.ToInt16(nmbMotorSpeed.Value);

            try
            {
                OPEN_ROBOT_Lib.SetOpenLoopVelocity(motorSpeed, -motorSpeed, commandTimeOut, numRetries);

                Add_Message("Move Command Recieved");
            }
            catch (Exception ex)
            {
                Add_Message (ex.ToString());
            }
        }

        /// <summary>
        /// Spin robot left from Vision tab.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bSpinLeft_Click(object sender, EventArgs e)
        {
            int motorSpeed;

            motorSpeed = Convert.ToInt16(nmbMotorSpeed.Value);

            try
            {
                OPEN_ROBOT_Lib.SetOpenLoopVelocity(-motorSpeed, motorSpeed, commandTimeOut, numRetries);

                Add_Message("Move Command Recieved");
            }
            catch (Exception ex)
            {
                Add_Message (ex.ToString());
            }
        }

        /// <summary>
        /// Stop robot from Vision tab.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bStopRobot_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.StopRobot(commandTimeOut, numRetries);
                Add_Message("Robot Stopped.");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Checkbox to determine if the SRV1 Camera Board is connected.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void ckbSRV1CamConnected_CheckedChanged(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SetSRV1CameraConnectionStatus(ckbSRV1CamConnected.Checked);
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// File menu Upload PIC18F4520 Hex File event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void mnuFileUploadPicHexFile_Click(object sender, EventArgs e)
        {
            imageTimer.Stop();

            if (openPicHexFile.ShowDialog() == DialogResult.Cancel) return;

            picHexFileName = openPicHexFile.FileName;

            hexDataReader = new Thread(new ThreadStart(ReadHexDataThread));

            hexDataReader.Start();

            hexThreadTimer.Enabled = true;
            hexThreadTimer.Start();
        }

        /// <summary>
        /// Thread to read and upload a new CCS C compiled hex file to OPEN-ROBOT's PIC18F4520.
        /// </summary>
        void ReadHexDataThread()
        {
            hexUploadError = "";

            if (File.Exists(picHexFileName) == false)
            {
                Add_Message("File Does Not Exist!");
                return;
            }

            int startTime = System.Environment.TickCount;

            StreamReader sr = new StreamReader(picHexFileName);

            string hexLine;
            int lineNum = 0;
            byte[] ackChar;
            bool readError = false;
            while (!sr.EndOfStream)
            {
                try
                {
                    hexLine = sr.ReadLine();

                    // Only send if a valid line of hex code.
                    if (hexLine[0] == ':')
                    {
                        if (ckbSRV1CamConnected.Checked) OPEN_ROBOT_Lib.SendSocketData("~" + hexLine, true, '\r');
                        else OPEN_ROBOT_Lib.SendSocketData(hexLine, true, '\r');

                        for (int i = 0; i < 3; i++)
                        {
                            ackChar = OPEN_ROBOT_Lib.GetSocketDataBytes(commandTimeOut, 1);
                            if (ackChar.Length == 0)
                            {
                                Add_Message("Error on ackChar.length");
                                readError = true;
                                sr.Close();
                                goto READ_ERROR;
                            }
                            // Verify returned char.
                            if (i == 0)
                            {
                                if (ackChar[0] != 19)
                                {
                                    readError = true;
                                    goto READ_ERROR;
                                }
                            }
                            else if (i == 1)
                            {
                                if (ackChar[0] != 6)
                                {
                                    readError = true;
                                    goto READ_ERROR;
                                }
                            }
                            else if (i == 2)
                            {
                                if (ackChar[0] != 17)
                                {
                                    readError = true;
                                    goto READ_ERROR;
                                }
                            }
                        }
                        lineNum++;
                        Add_Message("Line#" + lineNum.ToString() + " uploaded.");
                    }
                    else if (hexLine[0] == ';')
                    {
                        // Skip this line and do nothing since it is a comment.
                    }

                    // Return if a read error occurs, but user know first.
                    READ_ERROR:
                    if (readError == true)
                    {
                        hexUploadError = "Read Error! Hex file upload failed!\r\n" + "Close socket, power cycle robot, and try again";
                        sr.Close();
                        return;
                    }
                }
                catch (Exception ex)
                {
                    hexUploadError = ex.ToString();
                    sr.Close();
                    return;
                }
            }
            sr.Close();
            int uploadTime = System.Environment.TickCount - startTime;
            Add_Message("Upload Time (sec) = " + uploadTime.ToString());
        }

        /// <summary>
        /// Hex file upload thread timer. Let's user know when the hexReadDataThread dies.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void hexThreadTimer_Tick(object sender, EventArgs e)
        {
            hexThreadTimer.Stop();
            hexThreadTimer.Enabled = false;

            if (hexDataReader.IsAlive == false)
            {
                if (hexUploadError != "")
                {
                    Add_Message(hexUploadError);
                }
                else Add_Message("Hex file upload is complete.");
            }
            else
            {
                hexThreadTimer.Enabled = true;
                hexThreadTimer.Start();
            }
        }

        /// <summary>
        /// Event that fires when the user changes the commad time out value.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void nmbCmdTimeOut_ValueChanged(object sender, EventArgs e)
        {
            commandTimeOut = Convert.ToInt16(nmbCmdTimeOut.Value);
        }

        private void mnuFileTestWiFi_Click(object sender, EventArgs e)
        {
            maxWiFiTime = 0;
            numWiFiSamples = 0;
            wifiTimer.Enabled = true;
            wifiTimer.Start();
        }

        private void mnuFileEndWiFiTest_Click(object sender, EventArgs e)
        {
            wifiTimer.Stop();
            wifiTimer.Enabled = false;
            Add_Message("Max time = " + maxWiFiTime.ToString() + "ms");
            Add_Message("Number of samples = " + numWiFiSamples.ToString());
        }

        private void wifiTimer_Tick(object sender, EventArgs e)
        {
            wifiTimer.Stop();
            int startTime = System.Environment.TickCount;

            byte[] errInfo = OPEN_ROBOT_Lib.GetAnalogToDigitalSensorReadings((int)nmbCmdTimeOut.Value,0);

            int transTime = System.Environment.TickCount - startTime;

            if (errInfo.Length != 8)
            {
                Add_Message("ERROR READING FORM ROBOT!");
                Add_Message("Max time = " + maxWiFiTime.ToString() + "ms");
                Add_Message("Number of samples = " + numWiFiSamples.ToString());
                return;
            }
            Add_Message(transTime.ToString() + "ms");
            if (transTime > maxWiFiTime) maxWiFiTime = transTime;
            numWiFiSamples++;
            wifiTimer.Start();
        }

        /// <summary>
        /// Get PWM Offset button press event.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bGetPwmOffset_Click(object sender, EventArgs e)
        {
            try
            {
                byte[] pwmOffsets = OPEN_ROBOT_Lib.GetPwmOffsets((int)nmbCmdTimeOut.Value, 0);

                int leftOffset = pwmOffsets[0];
                int rightOffset = pwmOffsets[1];

                Add_Message("PWM Motor Offsets = " + leftOffset.ToString() + "," + rightOffset.ToString());
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Event to set left PWM motor offset.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bSetLeftOffset_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SetPwmOffset((int)numPwmOffset.Value, true, (int)nmbCmdTimeOut.Value, 0);

                Add_Message("Left PWM Motor Offset is set");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Event to set right PWM motor offset.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bSetRightOffset_Click(object sender, EventArgs e)
        {
            try
            {
                OPEN_ROBOT_Lib.SetPwmOffset((int)numPwmOffset.Value, false, (int)nmbCmdTimeOut.Value, 0);

                Add_Message("Right PWM Motor Offset is set");
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }

        /// <summary>
        /// Event to get firmware version for PIC18F4520.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void mnuFileGetFirmwareVersion_Click(object sender, EventArgs e)
        {
            try
            {
                string firmwareVersion = OPEN_ROBOT_Lib.GetFirmwareVersion((int)nmbCmdTimeOut.Value, numRetries);
                Add_Message(firmwareVersion);
            }
            catch (Exception ex)
            {
                Add_Message(ex.Message);
            }
        }        
    }
}